#include "i264e_common.h"
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif

static uint8_t *i264e_nal_escape_c(uint8_t *dst, uint8_t *src, uint8_t *end)
{
    if( src < end ) *dst++ = *src++;
    if( src < end ) *dst++ = *src++;
    while( src < end )
    {
        if( src[0] <= 0x03 && !dst[-2] && !dst[-1] )
            *dst++ = 0x03;
        *dst++ = *src++;
    }
    return dst;
}


/****************************************************************************
 * i264e_nal_encode:
 ****************************************************************************/
void i264e_nal_encode(i264e_t *h, uint8_t *dst, i264e_nal_t *nal)
{
    uint8_t *src = nal->p_payload;
    uint8_t *end = nal->p_payload + nal->i_payload;
    uint8_t *orig_dst = dst;

    if( h->common.param.b_annexb ) {
        if( nal->b_long_startcode )
            *dst++ = 0x00;
        *dst++ = 0x00;
        *dst++ = 0x00;
        *dst++ = 0x01;
    } else { /* save room for size later */
        dst += 4;
	}

    /* nal header */
    *dst++ = ( 0x00 << 7 ) | ( nal->i_ref_idc << 5 ) | nal->i_type;

    dst = h->common.bsf.nal_escape( dst, src, end );
    int size = (dst - orig_dst) - 4;

    /* Write the size header for mp4/etc */
    if( !h->common.param.b_annexb ) {
        /* Size doesn't include the size of the header we're writing now. */
        orig_dst[0] = size>>24;
        orig_dst[1] = size>>16;
        orig_dst[2] = size>> 8;
        orig_dst[3] = size>> 0;
    }

    nal->i_payload = size+4;
    nal->p_payload = orig_dst;
}

void i264e_bitstream_init( i264e_bitstream_function_t *pf )
{
    memset( pf, 0, sizeof(*pf) );
    pf->nal_escape = i264e_nal_escape_c;
}
